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(57) Abstract 

A method and apparatus for processing signals from a sensor system including a distributed Kalman filter utilizing 
distributed data processing techniques to determine various system states (e.g. position, velocity, attitude, etc). System, 
state processor (18) and sensor state processors (24, 26, 28) are in communication with each other and receive and calculate 
error state data. The system errors are fed back to the sensor device processor and both the system and instrument errors 
are fed back to a data collection processor to continually make corrections in the measurements to compensate for the er- 
ror estimation. 
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1 DISTRIBUTED KALMAN FILTER 

The present invention relates to a method and 
« apparatus for data estimation processing^ and more 

particularly/ to a Distributed Kalman Filter utilizing 
^ 5 distributed data processing techniques. 

Kalman filtering techniques have been developed 
primarily for estimating state parameters in dynamic systems. 
Kalman Filters have been used in many applications such as 
in control systems where real time measurements are not 
10 possible. One of the areas of technology where a Kalman 
Filter is of great importance is in avionics. 

Ther€ is an increasing demand being placed on 
tactical aircraft avionic systems and this demand is 
impacting on the performance of the navigation sub-systems. 
15 Present day aircraft utilize an inertial navigation system 
such as the Strapdown Attitude Heading Reference System 
(SAHRS) having a plurality of gyroscopes and accelerometers 
to sense the various parameters necessary for flight control. 
; Another system presently being implemented is the Global 
26 Positioning System (GPS) , which utilizes a series of ' 

eighteen statellites plus three active spares, each circling 
the earth twice a day in six orbital planes, which will 
conduct and transmit navigational signals to any location. 

Each of the above systems as stand alone systems 
25 have their own advantages and disadvantages. It has been 
determined that a combination of the GPS with an inertial 
navigation system* will* provide optimal- navig'ation." In 
an article entitled Integration of GPS With Inertial 
Navigation Systems,- by Cbxy *Jr/* Navigation:" Journal of the 
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1 Institute of Navigation^ Vol. 25^ No, 2, 1978, PP. 236-245, 
the author discloses the use of an integrated GPS-inertial 
filter configuration. Cox acknowledges that his filter is 
based on a high-order Kalman algorithm that presents problems 
5 in execution at a desired rate. In GPS/AHRS: A Synergistic 
Mix, by Sturza/ et al, NAECON 1984, May. 1984, pp. 339-348, 
there is also disclosed an integrated Kalman filter for 
combining GPS and SAHRS systems.' However, no description of 
the mo^el for implementing the integrated Kalman filter is 
10 disclosed. The integration of sensors described in the 

above systems utilize standard Kalman filtering techniques. 
However, in the development of mathematical descriptions of 
' the error behaviors, the size of the Kalman filter states 
will increase markedly, and would lead to a high order model 
-^•5 , of the system. It follows, that a large number of uncertain 
variables that contribute to the state of estimation errors, 
would require a huge computer processing power and. memory. 

Recent system literature concerning the sxibject of 
real time Kalman. Filtering in the problem of navigation ^ 
'integration contains two major approaches to handle l&rge. ' 
scale state estimation algorithms. In one approach, - 
considerable effort is made for reducing the order of the 
Kalman filter. Usually this effort has lead to a sub-optimal 
Kalman filter by partitioning the system states and filter 
25 matrices, and rewriting the filter equations in terms of the 
resulting set of lower order equations. To insist on reduced 
states that have a computational significance in the 
application, is to risk degrading filter performance. 

An alternative approach is the decentralized Kalman 
30 filter in which all sub- systems and their measurements are 
interconnected. The fundamental idea is to decompose the 
large system into sub- systems and then manipulate the smaller 
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1 sub-systems in such a way that the objectives of the 
overall system are met. Although the decentralized 
filter is stable, it is not well suited for state 
estimation. In addition, there is no mechanism for 
5 enforcing the interconnection constraints and there 
are no workable algorithms for a large scale system. 

The present invention is directed to a distributed 
Kalman filter (DKF) for processing signals from at 
least one sensor device for a system having at least 
10 one measurement instrument to provide specific system 

and instrument data comprising a sensor state processor 
for receiving instrument error state data from at least 
one sensor device processor and calculating sensor 

data? — ^ system—state processor coupled 

15 to said sensor state processor . for receiving system error 
state data from said sensor device processor, for . 
calculating system error data and for feeding said system 
error data back to said sensor device processor; and means 
for outputting the ciesired sysi^em data and for feeding 

20 back the error data to said at 'least one sensor * device 
processor. * • 

The present invention provides a method for 
the distributed data processing of signals from at 
least one sensor device for a system having at least 

25 one measurement instrument to provide specific device 
data, said distributed data processing being performed 
in a Kalman filter, said method comprising receiving 
instrument error state data from at least one sensor 
device processor and calculating sensor instrviment 

30 error data in a sensor state processor; receiving 

system error state data from said sensor device processor 
and calculating system error data in a system state 
processor; feeding said system error data back to said 
sensor device processor; and outputting the desired system 

35 data and feeding back the error data to said at least one 
sensor device processor. 
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The present invention is directed to a 
distributed Kalman filter (DKF> utilizing distributee^ 
data processing techniques- The DKF of the present 
invention is especially useful in integrated multi-sensor 
systems such as the SAHRS-GPS system. The DKF 
provides numerous benefits in solving the burden on 
computer time by allowing for greater computational 
capability resulting in improved accuracy/' speed 
and reliability- The DKF of the present invention 
is a universal filter that can be used to great 
benefit in the sensor systems for numerous devices. 
In addition to navigation the distributed Kalman 
filter can be used for processing data in radar/ imag^ 
pro5cessingr ^opticsr -television -or-any system at all 
^5 where noise presents a problem in determining real 
— time data measurements* Devices in which, the DKF 
would be employed includes aircraft/ spacecraft/ 
land and 'water vehicles/ television and cameras. 
The above are merely examples and the use of the 
DKF is in no way limited to. those recited above. 

Typically/ sensor systems include one or more 
sensors that collect data needed for the operation of the 
device/ such as navigating a vechicler identifying a 
target or focusing a camera. The necessary data is 
usually provided in various states. For example/ 
for navigation/ the states may consist of position/ 
velocity and attitude.^ These are called system states. 
In addition/ the operation of the sensor itself 
consists of several states. In the navigation 
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example, the sensor may be a gyroscope which has states that 
include alignment, coupling and drift. These are called 
instrument states. Errors are always present in the sensor 
system since exact measurements and data collection are 
subject to noise. The DKF estimates the error for all the 
states which is then fed back to a data collection processor 
to continually make corrections in the measurements to 
compensate for the error. 

More particularly, ther DKF of the present invention 
processes signals from at least one sensor device of a system 
to provide specific system and instrument data. A 
distributed Kalman filter includes a sensor state processor 
that receives instrument error state data from at least one 
sensor device processor and calculates sensor instrument 
error data. A system state processor is coupled to the 
sensor state processor and receives system state data from 
the sensor state processor and calculates system error data. 
The system state processor feeds the system error data back 
to the sensor state processor. The DKF includes means for 
outputting the desired system data and for feeding back the 
^^ror data to the sensor device processor. 

A distributed system is defined as any 
configuration of two or more processors, each with private 
memory, in which the computations performed in each processor 
utilizes the combined resources of the component machines. 
The amount of communication between the processors depends 
upon the nature of the multi-sensor system. The operating 
system within each processor determines a communications 
request and provides the necessary software linkage and 
signaling required for effective communications. The 
software to be processed^ by the distributed computing system 
consists of functional modules that collectively comprise the 
distributed program. 
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^ addition to the general embodiment of the DKF 

described above, three embodiments are disclosed in the 
GPS-SAHRS environment. Each of the SAHRS and GPS systems 
have corresponding instrument-'and system errors represented 
^ by a multiplicity of states to be described in detail in the 
description of the invention. One system is a SAHRS-aided 
GPS navigator wherein the DKF includes a. GPS state processor 
and a system state processor • The GPS processor provides 
data, for example, to compute range and range rates to the 

10 ^^^^ statellites from the Doppler shift of carrier frequency. 
This data is. fed through- the GPS state processor and system 
state processor as described with the general DKF. The SAHRS 
processor provides acceleration and velocity to aid the GPS 
processor and system state processor, 

15 si?J^5J«_As a GPS -aided SAHRS navigator 

which requires the DKF. to eistimate only the errors in the 
SAHRS and feedback these errors to recalibrate only the 
SAHRS. The GPS position and velocity measurements are both 
supplied through the SAHRS. The third system is a mixed 
SAHRS/.GPS navigator wherein the/ DKF - includes both a SAHRS 
state processor and a GPS state processor together with a 
system state processor that are interfaced using distributed 
processing techniques. The GPS provides range measurements 
and statellite data. The SAHRS provides acceleration and 

2^ velocity transformed to the navigation frame together with 
attitude data. The GPS navigator uses this information for 
signal reaquisition.. The SAHRS uses the GPS position and 
velocity updates for instrument alignment and calibration. 

Figure 1 is a block diagram of a prior art 
integrated Kalman filter. 
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1 Figure 2 is a block diagram of the distributed 

Kalman Filter (DKF) of the present invention.. Figure 3 
is a block diagram showing the DKF in a SAHRS/GPS 
environment. Figure '4 is a block diagram of a DKF 
5 in a mixed SAHRS/GPS system. Figure 5 is a block 
diagram of a DKl* in a SAHRS-aided GPS system. 
Figure 6 is a block diagram of A DKF in a GPS-aided 
SAHRS system. Figure 7a is a block diagram of a 
system, model of a prior art standard Kalman filter, 
10 Figure 7b is a block diagram of a system model of a 
DKF for the SAHRS/GPS mixed system. 

Referring now to the drawings. Figure 1 is a block 
diagram showing the prior art Kalman Filter arrangement in a 

typical multi-sensor -system.. Sensors 1 and 2 compute the • ' 

15 error state signals which are then fed into Kalman Filters 1 
and 2 respectively • ' In general, sensors 1 and 2 compute both 
system errors and sensor errors. After Kalmaji Filters 1 and 
• 2 process the system errors they feed them into the Kalman 
Filter 3 which further processes Ahe system errors. This 
20- type of situation appears a likely candidate for a' 

decentralized multirate Kalman filter. The prior art system 
is redundant by processing the same system errors in both 
Kalman Filters 1 and 2. Furthermore, the integration of 
Kalman Filters 1 and 2 by Kalman Filter 3 reduces calculation 
25 reliability. 

In the present invention, a single distributed 
Kalman filter (DKF) is utilized to process both the instrument 
and system errors which increases the amount of error states 
that can be processed. As shown in figure 2, the DKF 10 
30 includes at least two individual processors, processor 12 for 
instrument errors and processor 14 for. system errors. The DKF 
10 shown in figure 2 is coupled to a system having a single 
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sensor device processor 16 that can compute a plurality of 
state signals received from a multiplicity of sensors. The 
sensor device processor 16 -transmits the sensor data to the 
DKF 10 where it is processed by state processors 12 and 14. 
Typically, the sensor data is inputted to the instrument 
state processor 12 to process the instrument errors while the 
system error is fed to the system state processor 14 through 
the processor 12. Processor 14 computes the system error 
which is fed back to processor 12. The system and instrument 
^^^O" fed back to the sensor device processor 16 which 

then makes the necessary adjustments to the incoming state 
signals. 

The advantages of the present arrangement are that 
the instrument error processor is hot burdened: with filtering 
^5 the system state errors but filters only the instrument 

■ error's while the system state processor filters the system 
errors received from the sensors.. Therefore, less computing 
time and memory are needed due to the elimination of the 
redundancy of the system error processor operation. 
^0 ~-^yjt^jc'fi:^jti&rmor^, €ha size" of the hardware necessary to 

accommodate the system is reduced making it applicable for 
real time operation. 

In another embodiment of the present invention, a 
distributed Kalman Filter is utilized to integrate two sensor" 
25 systems. . In figure 3; there is shown a DKF 18 arranged to 
integrate data from a Strapdown Attitude. Heading Reference 
System (SAHRS) and a Global Positioning System (GPS). 

The SAHRS system includes aircraft rate and 
acceleration as inputs- Inertial body rate and acceleration 
30 data are sensed by an array of skewed inertial components. A 
. sensor redundancy algorithm is performed to select signals, 
to isolate failures, and to monitor 'performances. Sensor 
compensations such as bias, scale factor, and body bending 
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are aligned and the sensory information is resolved along the 
orthogonal body axes. The orthogonal rate data are corrected 
for the effects of earth rate and aircraft angular velocity 
over the earth's, surface to obtain the aircraft angular rates 
with respect to the local level coordinate frame. These 
rates are utilized to derive the direction cosines and. 
associated vehicle attitude and heading. 

The inertial body -axis accelerations are 
transformed to the local level frame, compensated for the - 
effects of gravity and Coriolis acceleration and integrated 
to obtain local level velocities. The level velocity is 
divided by the radius of the earth. to obtain the angular 
transport rates for compensation of the measured inertial 

„ .angular -rates. ^ - _ ^ _ 

The primary computation of the SAHRS processor 20 
is the determination of the direction cosine matrix that 
relates the aircraft coordinate system to the local level 
coordinate system. The resultant data are. not siaf f iciently 
accurate, specifically in terms of standoff error. The more 
stringent accuracy requirements for SAKRS dictate that th<s 
actual filter is to be designed using sensory outputs and 
blending the external reference data to estimate error 
sources. 

The basis for the GPS system is the information 
25 transmitted by each satellite. This information includes the 
satellite ephemeris and the time of transmittion of the 
signal. Transit time is proportional to range, so except for 
clock bias offset and atmospheric path distortion, the user 
has a measure of the range to the sending satellites.. These 
30 measurements are called pseudo-range because of the clock 

bias. Four simultaneous pseudo-range measurements suffice to 
allow the user to solve for four unknowns, namely the three 
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components of his position plus his clock bias. Knowing the 
effects of errors in initial position and initial time on the 
estimated Doppler shift of the received satellite signals, 
the receiver can determine the frequency that must be 
tracked, which is the "apparent" broadcast carrier frequency, 
usually with a phase-locked loop. Progressive incjreases in 
the tracking error and attendant reductions in the detector- 
gain lead to a complete loss of lock. In order to avoid loss 
of lock, to improve the Doppler estimate, arid to reduce the 
acquisition time the aiding data should be obtained directly 
from the SAHRS via the DKF, 

As shown in Figure 3, the DKF 18 includes a SAHRS 
sensor state processor 24-, a GFS ,sehsor state processor 26 
and a common system state processor 28, The SAHRS state 
15 -proc6s:sor-2 4 -calculates the-instrument error of the SAHRS 
system while passing the system error data to the system 
processor 28. Similarly, the GPS state processor calculates 
the instrument error of the GPS system and passes the system 
error to the system processor 28. The system error processor' 
20 28 passes the system error data back to the SAHRS and GPS 
processors 24 and 26 respectively. The DKF feeds the 
SAHRS and GPS error back to the respective sensor processors 
20 and 22. The DKF 18 provides the required data output 
which includes role, pitch , heading, velocity north, east 
25 and vertical, latitude, . longitude and altitude. 

Figure 4 shows another embodiment of present 
invention wherein the DKF 18 is used to integrate the data 
from four prbcessors. In addition to the SAHRS and GPS 
processors 20 and 22, there are also provided a reference 
sensor processor 30 and a satellite data processor 32.* The 
reference sensor processor 30 includes a magnetic heading 
reference sensor for determining pressure, altitude, and true 
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airspeed. To insure a bounded heading error in the presence 
of the SAHRS sensory errors, an external magnetic heading 
reference (flux valve) is selected. Flux valves are utilized 
to provide accurate long term heading. The flux valve data 
and gyro-driven heading data are combined via the filter to 
achieve both short- and long-term heading accuracy • The 
calculation of vertical velocity by the SAHRS algorithm 
requires an external reference to ensure stable velocity 
data. The accelerometer bias and imperfec-t- gravitational 
correction will result in an unbounded vertical velocity in a 
relatively short time. In order to bound the vertical 
velocity error, it is necessary, to. utilize pressure altitude. 
The local level velocities are utilized in the calculation of 
"the ^gular transport rates over the earth's surface. These 
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angular rates are transformed into projections along the 
vehicle body axes to compensate for the measured angular 
rates. Without the true airspeed as a reference velocity, 
the attitude and velocity errors will contain the Schuler 
oscillations in the presence of certain component errors. 

The processor 24 contains 33 states derivated from 
the SAHRS sensor error model. The gyro error model is given 
as the following five classes: 
Scale factor errors, three states; 
Misalignment coupling errors, six states; 
Bias errors, three states; 

Mass unbalance drift errors, three states; 
Random noise errors, three states. 

The model for the accelerometers can be described as the 
following classes: 

Scale factor errors, three states; 
Misalignment errors, six states; 
Bias errors, three states; 
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Random noise errors, tihree stia-tes. 

A g^lobal network of. satellites can be configured so 
that at least four different satellites are above the local 
horizon for almost every point on or near -the earth. The 
selection of these four satellites has a great influence on 
the accuracy of a navigation fix. The satellite data 
processor 32 selects the proper satellites. The satellite 
selection algorithm consists of the following four steps: 

Step one Select the first satellite with the 
largest elevation angle; 

Step two - Choose the second satellite near to the 
first one to 110 degrees; 

Step three - Determine the third satellite with 
bptimxam geometry for visibility; 

Step four - Select the last satellite with the 
property of the minimum geometric dilution of precision. 

The satellite motion algorithm determines the 
position of satellites by the satellite equations of motion. 
, These equations can: be expressed in Euler-Hill form, which is 
a rotating coordinate system defined by right ascension of 
ascending node, orbital inclination, and latitude. There 
exists an orthogonal matrix that transforms the position 
vector of a satellite in the Euler-Hill rotating form to the 
Cartesian coordinate of the inert ially fixed geocentric 
system. The purpose of this algorithm is to develop 
Lagrange's equations of satellite motion of a perturbing 
acceleration in the Euler-Hill rotating frame, in terms of 
the angular velocity vector and eccentricity vector, the 
nonsingular orbital elements V ranges and range rates are 
determined by the transformation. 

The processor 26 contains 10 states derived from 
the GPS sensor error model. They are three range 
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1 measurements states, three range rate states, one clock state 
and one clock rate state. The processor 28 contains 9 states 
derived from the aircraft attitude, position, and velocity • 
The Reconfiguration Data Management System 34 
5 includes algorithms to perform failure monitoring, failure 
isolation, configuration selection, and data normalization. 
In addition, analytic testing calculations are performed to 
minimize overall hardware requirements. the normalization 
computation proce.ss is the. final output- parameter 
10 computation, which uses best-estimate data to derive the 
output parameters. 

The GPS receiver provides pseudo-range and delta- 
range measurements, and satellite data. The SAHRS provides 

acceleration and velocity transformed to the navigation 

15 frame and attitude data. The GPS navigator uses this 

information for signal reacquisition following intervals for 
signal outages (resulting from antenna shadowing, bad 
geometry;' and high dynamic maneuvering) ♦ The SAHRS uses the 
GPS position and velocity updates for alignment and 
20 calibration of its instruments. ' The accurate position fixes 
from the satellite data can not only prevent long-term 
inertial error growth, but may allow various inertial errors 
to be estimated in real time and thus compensated for. The 
- error model of the filter is obtained by augmenting the state 
25 vector of the GPS-aided SAHRS error model by 10 elements. 

These 10 elements represent the range, range-rate, clock bias 
and clock rate of GPS correlated errors. The error model of 
the total states is 46 and the update interval is one second. 

Figure 5 shows the DKF 18 arranged as a GPS aided 
30 SAHRS navigator. One way of combating long-term inertial 

error growth from the SAHRS is to periodically reset the user 
position coordinates using an accurate fix from GPS. This 
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configuration requires the DKF to estimate only the errors in 
the SAHRS and feed back these errors to recalibrate only the 
SAHRS, The GPS position and velocity measurements are both 
supplied to the SAHRS, The system then represents the 
5 updated states that will be subsequently propagate 50 

iterations through time until the period of. a one second 
update cycle. A 3 6- state filter is implemented in the 
GPS-aided SAHRS navigation set. These error states consist 
of the six acceleration errors , nine gyro errors, 12 

10 misalignment errors of both accelerometers and gyros, and 
nine system errors « 

The system of Figure 6 shows the DKF 18 implemented 
as a SAHRS-aided GPS navigator. The. GPS receiver provides 
the data necessary to compute ranges and range-rates to the 

-^5 four satellites from the Doppler shift of carrier frequency^ 
There are two important errors that occur in making these 
range and range-rate measurements. The first one is caused 
by the user's clock not being perfectly synchronized with the 
sattelite clock System. The second error, is caused by an 

20 oscillator frequency error relative to the transictitted 
frequencies of the satellites. 

The SAHRS provides acceleration and velocity to aid 
the receiver in the phase- lock loop. The DKF is formed in a 
. two-stage process. The first stage estimates position from 

25 GPS pseudo-range measurements and velocity inputs. The 

second stage uses range-rate measurements and the output from 
the first stage, plus acceleration inputs. The filter 
formalism requires 16 error states; they are four range 
measurements, four range-rate measurements^ three gyro 

30 biases, three accelerometer biases, and the GPS receiver 
clock bias and bias rate. Range measurement residual is 
computed five times per second. The measure vector is based 
on the SAHRS cpmputation being available 50 times per second. 
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Algorithm design addresses not only the design of 
analytic estimation algorithms, but also the design of 
implemental procedures such as one whose function is to 
detect and respond to white noise in measurements. The 
design process includes mapping, these algorithms into a 
system of software procedures that, when executed on some 
target equipment, will interact correctly with the 
environment and among themselves, and will also satisfy the 
real-time constraints of the problem. 

The symbols and subscripts in the following 
discussion are defined as follows: For the i-th subsystem at 
the k-th update time, X. , = state vector, z. . = measure 
vector, Vj^^j^ white measurement noise vector, ]^ = input 
whi-te-r-noise-vector.'-' F .v , -=-the state "transition 'matrix from 
the j-th subsystem state vector to the i-th ' subsystem state 
vector. ^ij^k linear connection matrix from the i-th 

subsystem state vector to the j-th subsystem measure vector. 

In the development of .a distributed Kalman filter, 
the starting point is derived from the discreet system model • 
of standard Kalman equations; then, the partition is taken tp 
the desired subsystems- The system is described by the ' " 
following linear vector equation: 

/^k+1 = Vk-'^K- (1) 
Here, w^^ is the system noise and is a zero-mean white noise 
process with covariance: 

Cov ^Wj^^Wj J = Qic<y^'3# E[Wj^3«0 (2) 
in which Qy^ is a nonnegative definite matrix and <5i*j is the 
Dirac delta function. 

The subscript is a discrete filter update time 
argument that k,j^O. System equation is often referred to 
as the system model, since it describes the basic information 
that we are trying to determine. 
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The state vector, 
of a noisy mechanism of the 




is observed by means 



T 

k k- 



+ V, 



where the measurement noise Vj^ is 
process with: 
Gov 



. (3) 

.a zero-mean white noise 



E[Vj^l=0, 



(4) 



in which Rj^ is a nonnegative definite matrix. 
The measurement equations is called the observation 
For simplicity, w and v are assumed uncorrelated so • 
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is the sensor-one 
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^2,}^ sensor-two state vector, and 



^3,k 



into three substate vectors in which X 
state vector, 

system state vector. This scheme is depicted in Fig. 7 and 
forms a distributed computing system model. One of the 
differences between a distributed job and a conventional one 
is that a job may potentially execute on separate processors 
to provide coherence to a set of inputs. 
Then, 



(8) 
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(9) 



These forms are expanded and rewritten in the following three 
separate systems* 

Sensor-one state space equation 



10 



l,k+l - '^lUk-l^k * ^12,k^2,k -^ '^la/k^S^k * ^l,k 



+ F, 



^1,1. = «ll,k%-,k ^ «21,k%,k * «31,k%,k * -l,k 



15 



20 



Sensor-two state space equation: 



^2,k+l - ^22,k^2,k * ^21,k^l,k * ^22,k.^3,k * *'2,R 
22,k «22,k'^^2,k * «12,k%,k * «32,k%,k ^2, 



System state-space equations: 



(10) 
(11) 



(12) 
(13) 



^3,K+1 - '33,k'"3,k ^31,k*l.k '32,k'^2,k '^3,k 
•3,k = "33,k%,k ^13,k'^^l,k "23,k^^2,k ^3,k 



+ F,- , X- , + w. 



25 



(14) 
(15) 



Since sensor ^i^y. ^"^^2,k sl:ates are almost independent, let 
30 p. 



(16) 
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1 The standard Kalman filter is a linear, 

descrete-time finite dimensional system. The equations are 
summarized for convenience as follows; 

The filter is initialized by: 
5 Xq)^^ = Xq, and Pq[^^ p^. (17, 



The estimates are: 

^ iji A 

^k+l[k " Vk ^^kl k-1 ^ V}z' 

^k^ilk - ^kf^klk-i - Mk-A fV^k|.k-A 

V^V^kl-ii^k"' Qk- (19) 

The measurement update equations are: 

A A ' ' 



^klk-i : ^k|k-i * Pk|k.i«k f«k •'^ki k-i«k 



20 -o 1 ; . 

* ^)~^(Zk - ^k.'^klk-lJ . (21) 



^kjk = ^klk-1 - Pk|k-l«kfV^k[k-l«k 
25 " VXX|k-l (22) 

where, based on a set o£ sequential observations s 



= {^1' Z3 Z^} 

^lk-l = =f^k 1 VlJ' 



(23) 
(24) 
(25) 
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A further extension of the standard Kalman filter 
yields three nonlinear subfilters that are no longer linear 
and the performance is different from the original one. For 
some, partition of substate vectors may diverge and be 
effectively useless, whereas for other selections it may 
perform well. In order to ensure stability of the 
distributed Kalman filter for certain coordinate basic 
selections, one important property is to make sure that the 
individual processors can accomplish a global effect, 
executing code and data, and working together to complete an 
estimation . task. 

Three subsystem models: 



15 



^i,k+l 



^i,k 



^i,k<^i,k> * ^i,k 



*^i,kf.^i,k> 



(26) 
(27) 



20 



where the functions of fj^, hj^ are nonlinear, and i = 1,2, 
and 3. 



ii,k 



ax 



A 

X=X 



'k|k 



25 



H 



ii,k 



ah^^j^(X) 



9x 



A 

x=x 



k I k 



(28) 



where = partial derivative. 
20 Approximations are introduced to drive a clearly 

suboptimal filter for the model. 



fi,k<^k> = ^i,k''^k|k> * ^ii,k<^k - '^klk' * ••• 
35 »^i,kfV - ^i,k<'^k|k> * «ii,k<^k - ^k|k-l 



) + 



(29) 
(30) 
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^ Then the mod^l is as: 

5 2i/k = <«ii,kJ^i,k * ^i,k + ^i,k' -t^z) 

where ' ^ ^ - 

^l,k ~ ^l,k^^l,k|k^ ~ ^ll,k^l,k|k 

10 - ^12,k^2,k|k * ^13,k^3,k|k 

A A 

^2,k " ^2,k^^2,klk^ " ^22,k^2,k|k 

A ^ " 

^21,k^l,k|k ^23,k^3, kj k (34) 

15 A A 

- °3,k " ^,k^^3;k|k' " ^33,k^3,klk 
A A 
^31,k^l,k|k * ^32,k^2,kjk (35) 

A 'A 
^l,k ~ **irk^^l,k|k-l " ^ll,k^L,k|k-l 

A A 

* ^21,k'^2,kjk«l * ^31,k^3,k|k-l (36) 

A A 
25 ^2,k ' **2,k^^2,k|.k-l^ ' ^22,k^2,k|k-l 

A A 

*^.12,k^l,k|k-l ^32,k'^3,k|k-l (37) 

_ A A 

30 ^3,k ~ *^3,k^''^3,k|k-l' " ^33,k^3,k|k-l 

A A 

* ^13,fc'^l,k|k-l '^ "23,k^2,k|k-l (38) 
Extended Kalnan filter equations are: 
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20 



30 



35 



A A /V 

X 



+ H . 



i^kjk ' ^i,k|k-i * ^i,kfZi,k - <«ii,A,k|k-i 

A A 
i,i-l,k^i-l,k|k-l * "i,i-2,k^i-2,kjk-l (39) 

A _ ^ A 

^i,k|k-l - ^ii,k^i,k(k * ^i,i-l,k^i-l,k|k 

* ^i,i-2,kV2,klk <40) 

^i.k - ^i,kik-i«i,k<«ii,k''^;k|k-i«ii,k'-^ ^,k>"^ 

^^i^kjk = ^i^kjk-i ' Pi,k k-i"ii,kf"ii,k ^kjk-i"ii,k 

^ ^i,kJ ^^ii,k'^^k|k-l (42) 



^i,k+i|k = ^ii,kPi,k|k^ii,k <«) 



Figure 7a represents the continuous system model of 
a standard Kalman filter shown in Figure 1.- The states to be 
estimated must be modeled in the following vector form: 
X = F X + w 

The measurement relationship connecting the noisy measurement 
vector Z to the state vector X must be of the form: 
2 « H X + V 

The method of processing in channel 40, includes the input 
white noise vector, 

w = [Wj^,W2rW2]^ , 
combined in a combiner 42 with previous state vector 



T 

X = which has been multiplied in 

multiplier 44 by the linear connection matrix F in channel 

AS, to produce the derivative of the present state vector 

X ^ rx^,X2rX2l- 
The output is passed through an integrator 48 to produce 
present state vector X. The present state vector may go 
through channel 46 for re-input to combiner 42 and may stay 
on channel 40 for input to multiplier 50 to be multiplied by 
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15 



linear connection matrix H. The output of multipler 44 is 
combined in the combiner 4 2 for estimating the next state 
vector. The output of multipler 50 combines white measure 
noise sequence v = [v^^, Vj/V^ 1*^ in the combiner 52 to 
produce the present measurement vector 2 = [2^^,22,22!^ 
Based upon the system model in Fig. 7a, the equations of the 
standard Kalinan filter are presented in ecuations (17) to 
(25). 

Figure 7b shows the method of distributed . 
processing where the input white nose vector, w^^ is in 
channel 60, is in channel 62 ^nd is in channel 64. 
Processor 24 of the Fig. 7b shows the input white noise 
component w^, combined in a combiner 66 with "previous state 
vector Xj^,. which was multiplied in multiplier 68 by the 
linear connection matrix F^j^^ in channel 70, with previous 
state vector which was multiplied in multiplier 72 by the 
linear connection matrix F^2 channel 24, and with previous 
state vector X3, which was multiplied in multiplier 7 6 by the 
linear connection matrix F^-| in thfe channel 78 . The output 
from the combiner 66 produces the derivative df the present 
state vector, X^:. The vector X^ is passed through an • . 

integrator 80 to produce present state vector X^^. The. 
. present state vector X^^ will go through channel 70 and be 

multipled by F^^^ for re-input to combiner 66, stay on channel" 
25 60 and be multiplied by linear connection matrix Hj_^ in a 
multipler 82, and be sent to processors 26 and 28. The X 
from processor 28 is multiplied by H^^ in the multipler 88 of 
channel 90. The sum of the outputs from channels 60, 90, and 
86 are combined with white measure noise sequence, v in a 
combiner 92 to produce the present measurement component Z 
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1 Processor 26 of Fig, 7b shows the input white noise 

component ^^^^ combined in a combiner 94 with previous state 
vector X^, which was multiplied in multiplier 96 by the 
linear connection, matrix in channel 98, with previous 

5 state vector X^, which was multiplied in multiplier 100 by 
the linear connection matrix F^j in channel 102, and with 
previous state vector X^, which was multiplied in multiplier 
104 by the linear connection matrix F^^ ±n channel 106. The 
output from the combiner 94 produces the derivative of the 
10 present state vector, X^. The vector X2 is passed through an 
integrator 108 to produce present state vector X^ . The 
present state vector X^ will go through channel 98 to be 
multiplied by for re-input to combiner 94, stay on 

channel 64 and be multiplied by linear connection matrix H22 
15 in the multiplier 110, and is sent to processors 24 and 28. 
The vector X^ from processor 24 is multiplied by H^^ in' the 
multipler 112 of channel 114 and the vector from processor 
29 is multiplied by in the multiplier 116 of channel 118. 

The sum of the -butputs from channels 64, 118 and 114 are 
20 combined with white measure noise sequence, Vg in a 'combiner 
120 to .produce the present measurement component Z2. 

Processor 28 of the Fig. 7b shows the input white 
noise component W^, combined in a combiner 122 with previous 
state vector X^ , which was multiplied in multiplier 124 by 
25 the linear connection matrix F^^ i^i channel 126, with 

previous- state vector X2, which was multiplied in multiplier 
128 by the linear connection matrix F^j in channel 130, and 
with previous state vector X^, which was multiplied in 
multiplier 132 by the linear connection matrix F^^^ in the 
30 channel 134. The output from the combiner 122 produces the 

derivative of the present state vector, X^ . The vector is 
passed through an integrator 136 to produce present state 
vector, X^. The present state vector X^ will go through 
channel 126 to be multiplied by F^^ for re-input to combiner 

35 



wo 88/01409 



-24- 



PCT/US87/01946 



10 



122V stay on channel 62 to be multiplied by linear connection 
matrix H^g in the multipler 138, and be sent to processors 2 4 
and 2€. The vector from processor 24 is multiplied by H^^^ 
in the -multiplier 14Q of channel 142 and the vector from 
processor 26 is multiplied by H^^ in the multipler 144 of 
channel 146. The sum of the.outupts from, channels 62, 142, 
and 14 6 is combined with white measure noise sequence, v^ in 
a combiner 148 to produce the present measurement component 
Zg. Based upon the system mgdel in Fig. 7h, the equations of 
the distributed Kalman filter are implemented in accordance 
with equations f39) to (43). The dashed lines and nodes are 
represent optional choices. 

The system model of Figure 7b represents the 

."^^ ^^^Z.^^i^^ is implemented across a number of 
^5 physical devices that commuhicate with each other. The 

algorithm of the DKF operates on the system errors in order 
that they will be eliminated out of the system providing 
improved performance as. the end result. An advantage of the 
DKF 6f the present invention is an approximate 78% reduction 
in the total number of operations and 57% decrease in 
required computer memory. In the mixed SAKRS/GPS system* 
this results in the optimal combining of the excellent long 
term performance of GPS with the good short term performance 
of SAHRS. 

While illustrative embodiments of the subject 
invention have been described and illustrated, it is obvious 
that .various changes and modifications can be made therein 
without departing from the spirit of the. present invention 
which should be limited* only by the scope to the appended 

30 

claims . 
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i WHAT IS CLAIMED IS; 

1. A distributed Kalman filter for processing 
signals from at least one sensor device for a system 
having at least one measurement instrument to provide 

5 specific system and instrument <3ata comprising: 

a sensor state processor (12) for receiving 
instrument error state data from at least one sensor 
device processor (16) and calculating sensor instrument 
error data; 

10 a system state processor (14) coupled to said 

sensor state processor (12) for receiving system error 
state data from said sensor device processor (16)/ 
for calculating system error data and for feeding 
said "system -error data -back to said sensor- device 

■^5 processor (16); and 

means for outputting the desired system data 
and for feeding back the error data to said at least 
one sensor device processor (16). 

2. The distributed Kalman filter of Claim 1 
20 wherein said system is a navigation system/ such as 

a Strapdown Attitude Heading Reference System (SAHRS) 

or a Global Positioning System (GPS). 

3- The distributed Kalman filter of Claim 2 

wherein both of said SAHRS and GPS navigational systems 
^5 are coupled to said distributed Kalman filter. 

4. The distributed Kalman filter of Claims 1/ 

2 or 3 wherein said distributed Kalman filter network 

includes a SAHRS sensor state processor (24) and a 

GPS sensor state processor (26)/ both of said SAHRS 
30 and GPS sensor state processors being coupled to said 

system state processor (28). 
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5. The distributed Kalman filter of any one 
of the qrecedina claims wherein both of said sensor 
(24/26} and system (28) state processors include 
first means (66/94/122) for combining input signals 
having noise with a first sensor present state vector 
and a system present state vector to produce a derivative 
sensor vector and means (80/108/122) for integrating 
said derivative sensor vector to produce said sensor 
present; state vector/ and include means for combining 
a second sensor present state vector in said first com- 
bining means. 

6. The distributed Kalman filter of Claim 5 
wherein both of said sensor (24/26) and system (28) 

_ ^J^?.^?„.?j^^^?ss?f®_. ^^5:^^/3? ^i^st (68/96/128) and second 
-^-5 (76/100/132) means for multiplying both said first 
and second system present state vectors by first 
and second matrices prior to being combined la said 
first combining means and including .third means (72# 
104/124} for multipying said second present state 
vector by a third matrix prior to being combined in 
said first combining means. 

7. The distributed Kalman filter of Claims 5 
or 6 wherein both said sensor (24/26) and system (28) ' 
state processors include second means (92/120/148) 
for combining at least two of said present state 
vectors with a noise vector to produce a present measure- 
ment signal and wherein said first and second sensor 
present state vectors and said system present state 
vector are combined in second combining means. 

30 
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8. The distributed Kalman filter of Claims 5/ 
6 or 7 including means for multiplying each of said sensor, 
and system present state vector by first (82/110/146)/ 
second (88/116/138) and third (84/112/144) matrices 
respectively prior to being combined by said second 
combining means* 

9* A method for the distributed data processing of 
signals from at least one sensor device for a system having at 
least one measurement instrument to provide- specif ic""d-evice - ' 
data, said distributed data processing being performed in a 
Kalman filter, said method comprising: 

receiving instrument error state data from at least 
one sensor device processor and calculating sensor instrument 
error data in a. sensor state processor; 

receiving system error state data from said sensor 
device processor and calculating system error data. in a system 
state processor; 

feeding said system error data back to said sensor 
device processor; and 

outputting the desired system data an^^ feeding back 
the error data to said at least one sensor device processor . 

10. The method of Claim 13 wherein said system 
is a is a navigation system such as a Strapdown Attitude 
Heading Reference System (SAHRS) or a Global Positioning 
System (GPS) • 

11- The method of Claim 10 including coupling 
both a SAHRS and GPS navigational system to said distributed 
Kalman filter. 
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1 12. The method of Claims 9/ 10 or 11 wherein 

receiving and calculating instrument and system error 
state data includes combining input signals having noise 
with a first sensor present state vector and a system 
5 present state vector in a first .combining means to 

produce a derivative sensor vector and integrating said 
derivative sensor vector to produce said sensor present 
state vector and combining a second sensor present 
state vector in said first combining means. 
^0 13, The method of Claim 12 including mutiplying 

both said first and second system present state vectors 
by first and second matrices prior to being combined in 
said first combining means; and multiplying said second 
present state vector by a third matrix prior to being 
15 combined in said first combining means. 

14. The method- of Claims "12 or 13 including 
combining at least two of said present state vectors with 
a noise vector in a second combining means to= produce 
a. present measurement signal^ and multipying each of 
said sensor and system present state vectors by first/ 
second and third matrices respectively prior to being 
combined by said second combining means. 
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